#include "ros/ros.h"
#include "serial/serial.h"
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <ctime>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/package.h>
#include <opencv2/core.hpp>
#include <geometry_msgs/AccelStamped.h>
#include <list>


#define  BYTE0(dwTemp)       ( *( (uint8_t *)(&dwTemp)    )  )
#define  BYTE1(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 1) )
#define  BYTE2(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 2) )
#define  BYTE3(dwTemp)       ( *( (uint8_t *)(&dwTemp) + 3) )

typedef enum {
    UP_VERSION = 0x00,
    UP_STATUS = 0x01,
    UP_SENSER = 0x02,
    UP_RCDATA = 0x03,
    UP_GPSDATA = 0x04,
    UP_POWER = 0x05,
    UP_MOTOR = 0x06,
    UP_SENSER2 = 0x07,
    UP_FLYMODE = 0x0A,
    UP_SPEED = 0x0B,
    UP_PID1 = 0x10,
    UP_PID2 = 0x11,
    UP_PID3 = 0x12,
    UP_PID4 = 0x13,
    UP_PID5 = 0x14,
    UP_PID6 = 0x15,
    UP_RADIO = 0x40,
    UP_MSG = 0xEE,
    UP_CHECK = 0xEF,

    UP_REMOTER = 0x50,
    UP_PRINTF = 0x51,

    UP_USER_DATA1 = 0xF1,
    UP_USER_DATA2 = 0xF2,
    UP_USER_DATA3 = 0xF3,
    UP_USER_DATA4 = 0xF4,
    UP_USER_DATA5 = 0xF5,
    UP_USER_DATA6 = 0xF6,
    UP_USER_DATA7 = 0xF7,
    UP_USER_DATA8 = 0xF8,
    UP_USER_DATA9 = 0xF9,
    UP_USER_DATA10 = 0xFA,
} upmsgID_e;


#define  D_COMMAND_ACC_CALIB        0x01
#define  D_COMMAND_GYRO_CALIB        0x02
#define  D_COMMAND_MAG_CALIB        0x04
#define  D_COMMAND_BARO_CALIB        0x05
#define  D_COMMAND_ACC_CALIB_EXIT    0x20
#define  D_COMMAND_ACC_CALIB_STEP1    0x21
#define  D_COMMAND_ACC_CALIB_STEP2    0x22
#define  D_COMMAND_ACC_CALIB_STEP3    0x23
#define  D_COMMAND_ACC_CALIB_STEP4    0x24
#define  D_COMMAND_ACC_CALIB_STEP5    0x25
#define  D_COMMAND_ACC_CALIB_STEP6    0x26
#define  D_COMMAND_FLIGHT_LOCK        0xA0
#define  D_COMMAND_FLIGHT_ULOCK        0xA1

#define  D_ACK_READ_PID                0x01
#define  D_ACK_READ_VERSION            0xA0
#define  D_ACK_RESET_PARAM            0xA1

typedef enum {
    DOWN_COMMAND = 0x01,
    DOWN_ACK = 0x02,
    DOWN_RCDATA = 0x03,
    DOWN_POWER = 0x05,
    DOWN_FLYMODE = 0x0A,
    DOWN_PID1 = 0x10,//rpy angle rate
    DOWN_PID2 = 0x11,//rpy angle
    DOWN_PID3 = 0x12,//vx vy vz
    DOWN_PID4 = 0x13,//x y z
    DOWN_PID5 = 0x14,
    DOWN_PID6 = 0x15,
    DOWN_RADIO = 0x40,

    DOWN_REMOTER = 0x50,
    DOWN_POSITION = 0x60, //add by ljy
    DOWN_COMMANDERBITS = 0x70, //add by ljy
} downmsgID_e;
#define  POS_NEXT 1
#define  POS_NOW 0
#define  YAW_NEXT 2

#define COMMAND_BITS_POS_SP_MODE    0b11000110
#define COMMAND_BITS_POS_MODE       0b00000110
#define COMMAND_BITS_ALTI_MODE      0b00000101

#define  REFRESH_RATE       30
#define  START_WAIT_TIME    1.0f //second
#define  FLY_UP_TIME      4.0f //second
#define  START_WAIT_COUNT   (START_WAIT_TIME * REFRESH_RATE)
#define  FLY_UP_COUNT   (FLY_UP_TIME * REFRESH_RATE)

using namespace std;

typedef struct {
    float roll = 0;
    float pitch = 0;
    float yaw = 0;
    float thrust = 0;
    float trimPitch = 0;
    float trimRoll = 0;
    uint8_t ctrlMode = 0x10;
    bool flightMode = 0;
    bool RCLock = 0;
} remoterData_t;

typedef struct {
    float x = 0;
    float y = 0;
    float z = 0;
} pos_t;

typedef struct {
    pos_t currentPos;
    pos_t desiredPos;
} pos_now_next_t;

typedef struct {
    int16_t kp ;
    int16_t ki ;
    int16_t kd ;
} PID_t;


serial::Serial ros_ser;
std::vector<pos_t> csvdata;
int csv_ind = 0;
ofstream fly_distance_data;



void sendBySerial(uint8_t msgID, uint8_t data_0, uint8_t *pStruct, uint8_t StructSize) {
    uint8_t cmd[40];
//    uint8_t msgID = 0x50;
    cmd[0] = 0xAA;
    cmd[1] = 0xAF;
    cmd[2] = msgID;
    cmd[3] = StructSize + 1;
    cmd[4] = data_0;
    for (int i = 0; i < StructSize; i++)
    {
        cmd[5 + i] = pStruct[i];
    }
    uint8_t sum = 0;
    for (int i = 0; i < StructSize + 5; i++)
    {
        sum += cmd[i];
    }
    cmd[StructSize + 5] = sum;
    ros_ser.write(cmd, StructSize + 6);
    usleep(1000);
//    for (int i = 0; i < StructSize + 6; i++) {
//        cout << hex << int(cmd[i]) << "  ";
//    }
//    cout << dec << endl;
}


void sendBySerial(uint8_t msgID, uint8_t *pStruct, uint8_t StructSize) {
    uint8_t cmd[40];
//    uint8_t msgID = 0x50;
    cmd[0] = 0xAA;
    cmd[1] = 0xAF;
    cmd[2] = msgID;
    cmd[3] = StructSize;
    for (int i = 0; i < StructSize; i++)
    {
        cmd[4 + i] = pStruct[i];
    }
    uint8_t sum = 0;
    for (int i = 0; i < StructSize + 4; i++)
    {
        sum += cmd[i];
    }
    cmd[StructSize + 4] = sum;
    ros_ser.write(cmd, StructSize + 5);
    usleep(1000);

    for (int i = 0; i < StructSize + 5; i++)
    {
        cout << hex << int(cmd[i]) << "  ";
    }
    cout << dec << endl;
    cout << int(cmd[3]) << endl;

}

void sendBySerial(uint8_t msgID, uint8_t data_0)
{
    void* p;
    sendBySerial( msgID,  data_0, (uint8_t *)p,0);
}

void rotateDeltaYaw(float deltaYawToRotate)
{
    void* p;
    p = &deltaYawToRotate;
    sendBySerial(DOWN_POSITION,YAW_NEXT,(uint8_t *)p, sizeof(deltaYawToRotate));
    usleep(5000e3);
}

void read_serial() {
    std::string buffer;
    buffer = ros_ser.read(ros_ser.available());
    char msg_len = 0;
    char msg_type = 0;
    int i = 0;
    cout << "buffer:  " << buffer.size() << endl;
}

void initSendPID() {
    PID_t pidrr = {.kp=3000, .ki=0, .kd=65,};
    PID_t pidrp = {.kp=3000, .ki=0, .kd=65,};
    PID_t pidryaw = {.kp=2000, .ki=185, .kd=0,};
    PID_t pidr = {.kp=80, .ki=0, .kd=0,};
    PID_t pidp = {.kp=80, .ki=0, .kd=0,};
    PID_t pidyaw = {.kp=200, .ki=0, .kd=15,};

    PID_t pidvx = {.kp=45, .ki=0, .kd=0,};
    PID_t pidvy = {.kp=45, .ki=0, .kd=0,};
    PID_t pidvz = {.kp=1000, .ki=1500, .kd=100,};
    PID_t pidx = {.kp=40, .ki=0, .kd=6,};
    PID_t pidy = {.kp=40, .ki=0, .kd=6,};
    PID_t pidz = {.kp=60, .ki=0, .kd=45,};

    uint16_t thrustbase = 40000;
    int16_t trimR = 0;
    int16_t trimP = 0;
//
//    pidvz.kp = 2750;
//    pidvz.kd = 3000;
//    pidvz.ki = 20;
//    pidz.kp = 50;
//    pidz.kd = 0;
//    pidz.ki = 0;
//
    pidvx.kp = 50;
    pidvx.kd = 0;
    pidvy.kp = 50;
    pidvy.kd = 0;

    pidx.kp = 150;
    pidy.kp = 150;
    pidx.kd = 0;
    pidy.kd = 0;
    pidx.ki = 5;
    pidy.ki = 5;



    int pid_size = sizeof(PID_t);
    uint8_t temprate[pid_size * 3];
    uint8_t tempangle[pid_size * 3];
    uint8_t tempv[pid_size * 3];
    uint8_t temp[pid_size * 3];
    uint8_t tempthrutrim[sizeof(uint16_t) * 3];
    for (int i = 0; i < pid_size; i++)
    {
        temprate[i] = *((uint8_t *) (&pidrr) + i);
        temprate[pid_size + i] = *((uint8_t *) (&pidrp) + i);
        temprate[pid_size * 2 + i] = *((uint8_t *) (&pidryaw) + i);
        tempangle[i] = *((uint8_t *) (&pidr) + i);
        tempangle[pid_size + i] = *((uint8_t *) (&pidp) + i);
        tempangle[pid_size * 2 + i] = *((uint8_t *) (&pidyaw) + i);

        tempv[i] = *((uint8_t *) (&pidvx) + i);
        tempv[pid_size + i] = *((uint8_t *) (&pidvy) + i);
        tempv[pid_size * 2 + i] = *((uint8_t *) (&pidvz) + i);
        temp[i] = *((uint8_t *) (&pidx) + i);
        temp[pid_size + i] = *((uint8_t *) (&pidy) + i);
        temp[pid_size * 2 + i] = *((uint8_t *) (&pidz) + i);
    }
    tempthrutrim[0] = *((uint8_t *) (&thrustbase) + 0);
    tempthrutrim[1] = *((uint8_t *) (&thrustbase) + 1);
    tempthrutrim[2] = *((uint8_t *) (&trimR) + 0);
    tempthrutrim[3] = *((uint8_t *) (&trimR) + 1);
    tempthrutrim[4] = *((uint8_t *) (&trimP) + 0);
    tempthrutrim[5] = *((uint8_t *) (&trimP) + 1);
    sendBySerial(DOWN_PID1, temprate, sizeof(temprate));
    usleep(1000);
    sendBySerial(DOWN_PID2, tempangle, sizeof(tempangle));
    usleep(1000);
    sendBySerial(DOWN_PID3, tempv, sizeof(tempv));
    usleep(1000);
    sendBySerial(DOWN_PID4, temp, sizeof(temp));
    usleep(1000);
    sendBySerial(DOWN_PID5, tempthrutrim, sizeof(tempthrutrim));
    usleep(1000);
    ROS_INFO_STREAM("initSendPID end.");
}


pos_t init_pos;
bool cam_pos_inited = false;
bool vicon_pos_inited = false;

geometry_msgs::PoseStamped cam_origin,cam_reset_origin_pos;
geometry_msgs::PoseStamped vicon_origin,vicon_reset_origin_pos;
geometry_msgs::PoseStamped pos_setpoint_msg;
ros::Time start_timepoint;
geometry_msgs::PoseStamped drone_pos_msg;
geometry_msgs::TwistStamped drone_vel_msg;
geometry_msgs::AccelStamped drone_acc_msg;

//void camposcallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
//    pos_t current_pos_temp, current_pos;
//    current_pos_temp.x = (float) msg->pose.position.x;
//    current_pos_temp.y = -(float) msg->pose.position.y;
//    current_pos_temp.z = (float) msg->pose.position.z;
//    if (cam_pos_inited)
//    {
//        current_pos.x = current_pos_temp.x - init_pos.x;
//        current_pos.y = current_pos_temp.y - init_pos.y;
//        current_pos.z = current_pos_temp.z - init_pos.z;
//
//        cam_reset_origin_pos = *msg;
//        cam_reset_origin_pos.header.stamp = ros::Time::now();
//        cam_reset_origin_pos.pose.position.x -= cam_origin.pose.position.x;
//        cam_reset_origin_pos.pose.position.y -= cam_origin.pose.position.y;
//        cam_reset_origin_pos.pose.position.z -= cam_origin.pose.position.z;
//    } else
//    {
//        cam_origin = *msg;
//        init_pos = current_pos_temp;
//        current_pos.x = 0;
//        current_pos.y = 0;
//        current_pos.z = 0;
//        cam_pos_inited = true;
//    }
//
//
//    printf("current_pos: %7.3f%7.3f%7.3f\n", current_pos.x, current_pos.y, current_pos.z);
////    cout << setprecision(3) << current_pos.x << " " << current_pos.y << " " << current_pos.z << endl;
//    sendBySerial(DOWN_POSITION, POS_NOW, (uint8_t *) &current_pos, sizeof(current_pos));
////        sendBySerial(DOWN_POSITION,POS_NEXT,(uint8_t *) &desired_pos,sizeof(desired_pos));
//    usleep(1000);
//}

void viconposcallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {

    if (vicon_pos_inited)
    {

        vicon_reset_origin_pos = *msg;
        vicon_reset_origin_pos.header.stamp = ros::Time::now();
        vicon_reset_origin_pos.pose.position.x -= vicon_origin.pose.position.x;
        vicon_reset_origin_pos.pose.position.y -= vicon_origin.pose.position.y;
        vicon_reset_origin_pos.pose.position.z -= vicon_origin.pose.position.z;
    } else {
        vicon_origin = *msg;
        vicon_pos_inited = true;
    }
}



void droneposcallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
    drone_pos_msg = *msg;
}

void dronevelcallback(const geometry_msgs::TwistStamped::ConstPtr &msg) {
    drone_vel_msg = *msg;
}

void droneacccallback(const geometry_msgs::AccelStamped::ConstPtr &msg) {
    drone_acc_msg = *msg;
}

void goToPosition(pos_t posToGo)
{
    posToGo.y *= -1;
    sendBySerial(DOWN_POSITION, POS_NEXT, (uint8_t *) &posToGo, sizeof(posToGo));
}

void takeOff()
{
    sendBySerial(DOWN_COMMANDERBITS,COMMAND_BITS_POS_MODE);
    usleep(3000e3);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "fly_distance_test");
    ros::NodeHandle nodeHandle;

    std::string dev_name = "/dev/ttyACM0";
    int baudrate = 500000;
    ros::Rate loop_rate(REFRESH_RATE);

    std::string pkg_path = ros::package::getPath("drone_pnp");

    FILE *fp;
    fp = fopen((pkg_path + "/data/8-traj.csv").c_str(), "r");
    if (fp)
    { std::cout << "open path csv" << std::endl; }
    while (1)
    {
        pos_t pointtemp;
        fscanf(fp, "%f,%f,%f", &pointtemp.x, &pointtemp.y, &pointtemp.z);
        csvdata.push_back(pointtemp);
        if (feof(fp))break;
    }
    fclose(fp);

    fly_distance_data.open(pkg_path+"/data/fly_distance_data.csv");

    try
    {
        ros_ser.setPort(dev_name);
        // ros_ser.setPort("/dev/serial/by-id/usb-STMicroelectronics_STM32_Virtual_COM_Port_6D87155F5056-if00");
        ros_ser.setBaudrate(baudrate);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ros_ser.setTimeout(to);
        ros_ser.open();
    }
    catch (serial::IOException &e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
    }
    if (ros_ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port opened");
    } else
    {
        ROS_ERROR_STREAM("No port opened");
    }

    initSendPID();

////  control joystick
//    sendBySerial(DOWN_COMMAND,D_COMMAND_FLIGHT_ULOCK);
//
//    remoterData_t remoterData;
//    remoterData.roll=0; //deg
//    remoterData.pitch=0;
//    remoterData.yaw=0;  //-200 to 200
//
//    remoterData.thrust=0.0f; // 0 to 100
//    remoterData.trimPitch=3.3;
//    remoterData.trimRoll=1.76;
//    remoterData.ctrlMode=0x10;
////        remoterData.flightMode=true;
////        remoterData.RCLock=true;
//    sendBySerial(DOWN_REMOTER,1,(uint8_t *) &remoterData,sizeof(remoterData));
//    usleep(100e3);
//
//
//    sendBySerial(DOWN_COMMANDERBITS,COMMAND_BITS_ALTI_MODE);
//
//    usleep(5000e3);
//
//
//    ros::Rate rate(20);
//
//    while (ros::ok())
//    {
////        uint8_t commanderbits;
////        commanderbits = 0b10000000;
//
//        ros::Time startT = ros::Time::now();
//        while(ros::ok() && (ros::Time::now()-startT).toSec() < 1)
//        {
//            remoterData.thrust=50.0f; // 0 to 100
//            remoterData.pitch= 5;  //-200 to 200
//            sendBySerial(DOWN_REMOTER,1,(uint8_t *) &remoterData,sizeof(remoterData));
////            usleep(200e3);
//            rate.sleep();
//        }
//
//        ros::Time startT2 = ros::Time::now();
//        while(ros::ok() && (ros::Time::now()-startT2).toSec() < 1)
//        {
//            remoterData.thrust=50.0f; // 0 to 100
//
//            remoterData.pitch= -5;  //-200 to 200
//            sendBySerial(DOWN_REMOTER,1,(uint8_t *) &remoterData,sizeof(remoterData));
////            usleep(200e3);
//            rate.sleep();
//        }
//
//    }


    //todo

//    ros::Subscriber cam_pos_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/cam/pose", 1, camposcallback);
    ros::Subscriber vicon_pos_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/mocap/pose", 1, viconposcallback);
    ros::Subscriber drone_pos_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/fused_pos/pose", 1,
                                                                                     droneposcallback);
    ros::Subscriber drone_vel_sub = nodeHandle.subscribe<geometry_msgs::TwistStamped>("/fused_pos/vel", 1,
                                                                                     dronevelcallback);
    ros::Subscriber drone_acc_sub = nodeHandle.subscribe<geometry_msgs::AccelStamped>("/fused_pos/acc", 1,
                                                                                      droneacccallback);
    list<geometry_msgs::AccelStamped> accList;
    ros::Time start_hit_timestamp = ros::Time::now();
    int hit_count = 0;
    bool is_hitting = false;

//    ros::Publisher cam_reset_origin_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/cam_reset_origin/pose", 1);
//    ros::Publisher vicon_reset_origin_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/vicon_reset_origin/pose", 1);
//    ros::Publisher pos_setpoint_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/pos_setpoint/pose", 1);


    ros::spinOnce();

//    ros::spin();
    int j = 0, csv_ind = 0;
    start_timepoint = ros::Time::now();

    takeOff();

//    //rotate yaw
//    while(ros::ok())
//    {
//        rotateDeltaYaw(45);
//        usleep(5000e3);
//        j++;
//        if(j==20)
//            return 0;
//    }

    fly_distance_data << ",ViconPosition,,, ,PositionSetpoint,,, ,PositionByDrone,,, ,VelByDrone,,, ,AccByDrone,,," << endl;
    while(ros::ok())
    {
        j++;
        pos_t  desired_pos;
        desired_pos.x = 0;
        desired_pos.y = 0;
        desired_pos.z = 0.8f;

        if(csv_ind < csvdata.size())
        {
            desired_pos = csvdata[csv_ind++];
            desired_pos.x *= 0.5;
            desired_pos.y *= 1;
            desired_pos.z = .8;
        }
//        double sec_num = j%(16*REFRESH_RATE)*1.0/REFRESH_RATE;
//        desired_pos.x = 0;
//        desired_pos.y = sec_num < 3? sec_num/3*1 :(sec_num < 8? 1 : (sec_num < 11? 1 - (sec_num-8)/3*1 : 0));
//        desired_pos.z = 1.2f;


//            cout << desired_pos.x << "  " << desired_pos.y << "  " << desired_pos.z << endl;
        double sec_num = j%(16*REFRESH_RATE)*1.0/REFRESH_RATE;
        desired_pos.x = sec_num<3?sec_num*1.f/3.f:1;
        desired_pos.y = 0;
        desired_pos.z = 0.8f;

        double accx,accy, current_acc_strength, angleOfHit;
        accx = drone_acc_msg.accel.linear.x;
        accy = drone_acc_msg.accel.linear.y;
        current_acc_strength = accx*accx + accy*accy;
        angleOfHit = atan2(-accy,-accx) * 180 / M_PI;

        double directionAngle = atan2(desired_pos.y - drone_pos_msg.pose.position.y,desired_pos.x - drone_pos_msg.pose.position.x) * 180 / M_PI;

        double angleDifference;
        angleDifference = angleOfHit - directionAngle;
        angleDifference = angleDifference > 180 ? angleDifference - 360 : angleDifference;
        angleDifference = angleDifference < -180 ? angleDifference + 360 : angleDifference;

        if(current_acc_strength > 1.5*1.5 && abs(angleDifference) < 90 && !is_hitting)
        {
            hit_count++;
            start_hit_timestamp = drone_acc_msg.header.stamp;
            ROS_WARN_STREAM("accx: " << accx << " accy: " << accy <<" angleOfHit: " << angleOfHit << " directionAngle: " << directionAngle);
            accList.clear();
            is_hitting = true;
            desired_pos.x = drone_pos_msg.pose.position.x - 0.2;
            goToPosition(desired_pos);
        }

        if(is_hitting)
        {
            accList.push_back(drone_acc_msg);
            if((drone_acc_msg.header.stamp - start_hit_timestamp).toSec() > 0.9)
            {
                is_hitting = false;
                double maxAccx=0,maxAccy=0;
                for (auto i : accList)
                {
                    if (abs(i.accel.linear.x) > abs(maxAccx))
                        maxAccx = i.accel.linear.x;
                    if (abs(i.accel.linear.y) > abs(maxAccy))
                        maxAccy = i.accel.linear.y;
                }
                double acc_strength = maxAccx * maxAccx + maxAccy * maxAccy;
                ROS_WARN_STREAM("maxAccx: " << maxAccx << " maxAccy: " << maxAccy <<" hit_strength: "<< acc_strength);
                usleep(20000e3);
            }
        } else{
            goToPosition(desired_pos);
        }




//
//        if(accList.size() < 5)
//            accList.push_back(drone_acc_msg);
//        else{
//
//            accList.push_back(drone_acc_msg);
//            accList.pop_front();
//
//            if((accList.back().header.stamp - start_hit_timestamp).toSec() > 1)
//            {
//                double maxAccx = 0, maxAccy = 0;
//                for (auto i : accList)
//                {
//                    if (abs(i.accel.linear.x) > abs(maxAccx))
//                        maxAccx = i.accel.linear.x;
//                    if (abs(i.accel.linear.y) > abs(maxAccy))
//                        maxAccy = i.accel.linear.y;
//                }
//
//                stringstream ss;
//                ss << "accx: " << maxAccx << " accy: " << maxAccy;
//                double acc_strength = maxAccx * maxAccx + maxAccy * maxAccy;
//                double angle;
//                if (acc_strength > 1.5 * 1.5)
//                {
//                    angle = atan2(maxAccy, -maxAccx) * 180 / M_PI;
//                    ss << " Hit " << hit_count<< " Angle: " << angle <<"  Strength: " << acc_strength;
//                    hit_count++;
//                    last_hit_timestamp = accList.back().header.stamp;
//                    ROS_WARN_STREAM(ss.str());
//                }
//                ss << endl;
//                ROS_INFO_STREAM_THROTTLE(0.2,ss.str());
//            }
//        }



        pos_setpoint_msg.header.stamp = ros::Time::now();
        pos_setpoint_msg.pose.position.x = desired_pos.x;
        pos_setpoint_msg.pose.position.y = desired_pos.y;
        pos_setpoint_msg.pose.position.z = desired_pos.z;

//        double cam_time = (cam_reset_origin_pos.header.stamp - start_timepoint).toSec();
//        double vicon_time = (vicon_reset_origin_pos.header.stamp - start_timepoint).toSec();

        fly_distance_data << (vicon_reset_origin_pos.header.stamp - start_timepoint).toSec() <<","
                << vicon_reset_origin_pos.pose.position.x << ","
                << vicon_reset_origin_pos.pose.position.y << ","
                << vicon_reset_origin_pos.pose.position.z << ","
//                << cam_time <<","
//                << cam_reset_origin_pos.pose.position.x << ","
//                << cam_reset_origin_pos.pose.position.y << ","
//                << cam_reset_origin_pos.pose.position.z << ","
                << (pos_setpoint_msg.header.stamp - start_timepoint).toSec() <<","
                << pos_setpoint_msg.pose.position.x << ","
                << pos_setpoint_msg.pose.position.y << ","
                << pos_setpoint_msg.pose.position.z <<",";

        fly_distance_data << (drone_pos_msg.header.stamp - start_timepoint).toSec() << "," << drone_pos_msg.pose.position.x << "," << drone_pos_msg.pose.position.y << "," << drone_pos_msg.pose.position.z << ",";
        fly_distance_data << (drone_vel_msg.header.stamp - start_timepoint).toSec() << "," << drone_vel_msg.twist.linear.x << "," << drone_vel_msg.twist.linear.y << "," << drone_vel_msg.twist.linear.z << ",";
        fly_distance_data << (drone_acc_msg.header.stamp - start_timepoint).toSec() << "," << drone_acc_msg.accel.linear.x << "," << drone_acc_msg.accel.linear.y << "," << drone_acc_msg.accel.linear.z << ",";




        fly_distance_data << endl;
//        vicon_reset_origin_pub.publish(vicon_reset_origin_pos);
//        cam_reset_origin_pub.publish(cam_reset_origin_pos);
//        pos_setpoint_pub.publish(pos_setpoint_msg);

        loop_rate.sleep();
        ros::spinOnce();
//        cout << endl << endl;

    }



    ////old fly 8-traj
//    while (ros::ok())
//    {
////        uint8_t cmd[]={0xAA,0xAF,0x50,0x11,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
////        uint8_t cksum = 0xCA;
////        cmd[2] = cmd[2];
////        int cnt = 5;
////        float tmp;
////        for(int i = 0; i<4; ++i){
////            tmp = rpyt[i];
////            cmd[cnt++] = BYTE0(tmp);
////            cmd[cnt++] = BYTE1(tmp);
////            cmd[cnt++] = BYTE2(tmp);
////            cmd[cnt++] = BYTE3(tmp);
////            cksum += BYTE3(tmp) + BYTE2(tmp) + BYTE1(tmp) + BYTE0(tmp);
////            // printf("%02hhX %02hhX %02hhX %02hhX ", BYTE0(tmp), BYTE1(tmp), BYTE2(tmp), BYTE3(tmp));
////        }
////        cmd[cnt] = cksum;
////        // printf("%02hhX\n",cksum);
////        ros_ser.write(cmd,22);
////        // cout<<"rpyt_cmd = "<<rpyt[0]<<" "<<rpyt[1]<<" "<<rpyt[2]<<" "<<rpyt[3]<<endl;
//
//
////        remoterData_t remoterData;
////        remoterData.roll=0;
////        remoterData.pitch=0;
////        remoterData.yaw=0;
////
////        remoterData.thrust=20.0f;BYTE3
////        remoterData.trimPitch=0;
////        remoterData.trimRoll=0;
//////        remoterData.ctrlMode=0;
//////        remoterData.flightMode=true;
//////        remoterData.RCLock=true;
////        sendBySerial(DOWN_REMOTOR,1,(uint8_t *) &remoterData,sizeof(remoterData));
////        cout << sizeof(remoterData) << endl;
////        cout<<endl;
//
//        if (j > START_WAIT_COUNT)
//        {
//            pos_t current_pos, desired_pos;
//            current_pos.x = 0;
//            current_pos.y = 0;
//            current_pos.z = 0;
//            desired_pos.x = 0;
//            desired_pos.y = 0;
//            desired_pos.z = j > START_WAIT_COUNT+FLY_UP_COUNT ? 0.6f : (j - START_WAIT_COUNT) / FLY_UP_COUNT * 0.6f;
//            pos_now_next_t pos_now_next;
//            pos_now_next.currentPos = current_pos;
//            pos_now_next.desiredPos = desired_pos;
//
//            if(j > START_WAIT_COUNT+FLY_UP_COUNT+ REFRESH_RATE*3 && csv_ind < csvdata.size())
//            {
//                desired_pos = csvdata[csv_ind++];
//                desired_pos.x *= 0.5;
//                desired_pos.y *= 1;
//                desired_pos.z = .6;
//            }
//
////            sendBySerial(DOWN_POSITION, POS_NOW, (uint8_t *) &current_pos, sizeof(current_pos));
////            usleep(1000);
//            sendBySerial(DOWN_POSITION, POS_NEXT, (uint8_t *) &desired_pos, sizeof(desired_pos));
////            usleep(1000);
////            cout << desired_pos.x << "  " << desired_pos.y << "  " << desired_pos.z << endl;
//            pos_setpoint_msg.header.stamp = ros::Time::now();
//            pos_setpoint_msg.pose.position.x = desired_pos.x;
//            pos_setpoint_msg.pose.position.y = -desired_pos.y;
//            pos_setpoint_msg.pose.position.z = desired_pos.z;
//        }
//        j++;
////        if (j == 399)
////            j = 0;
//
//        double cam_time = (cam_reset_origin_pos.header.stamp - start_timepoint).toSec();
//        double vicon_time = (vicon_reset_origin_pos.header.stamp - start_timepoint).toSec();
//
//
//        fly_distance_data << vicon_time <<","
//                << vicon_reset_origin_pos.pose.position.x << ","
//                << vicon_reset_origin_pos.pose.position.y << ","
//                << vicon_reset_origin_pos.pose.position.z << ","
//                << cam_time <<","
//                << cam_reset_origin_pos.pose.position.x << ","
//                << cam_reset_origin_pos.pose.position.y << ","
//                << cam_reset_origin_pos.pose.position.z << ","
//                << pos_setpoint_msg.pose.position.x << ","
//                << pos_setpoint_msg.pose.position.y << ","
//                << pos_setpoint_msg.pose.position.z
//                <<endl;
//        vicon_reset_origin_pub.publish(vicon_reset_origin_pos);
//        cam_reset_origin_pub.publish(cam_reset_origin_pos);
//        pos_setpoint_pub.publish(pos_setpoint_msg);
//
//        loop_rate.sleep();
//        ros::spinOnce();
////        cout << endl << endl;
//    }


    return 0;
}
